2023-01-22
An ellipse is defined such that the sum of distances from any point on its curve to its two foci is constant. Using this, we can derive the formula for an ellipse with focal length \(c\) and semi-major axis \(a\). We will use the 2D cartesian coordinates \((\rho, z)\).
\[\begin{aligned} \sqrt{(\rho+c)^2 + z^2} + \sqrt{(\rho-c)^2 + z^2} &= 2a \\ \sqrt{(\rho+c)^2 + z^2} &= 2a - \sqrt{(\rho-c)^2 + z^2} \\ (\rho+c)^2 + z^2 &= 4a^2 - 4a\sqrt{(\rho-c)^2 + z^2} + (\rho-c)^2 + z^2 \\ (\rho+c)^2 &= 4a^2 - 4a\sqrt{(\rho-c)^2 + z^2} + (\rho-c)^2 \\ 4a\sqrt{(\rho-c)^2 + z^2} &= 4a^2 + (\rho-c)^2 - (\rho+c)^2 \\ 4a\sqrt{(\rho-c)^2 + z^2} &= 4a^2 - 4c\rho \\ \sqrt{(\rho-c)^2 + z^2} &= a - \frac{c}{a}\rho \\ (\rho-c)^2 + z^2 &= a^2 - 2c\rho + \frac{c^2}{a^2}\rho^2 \\ \rho^2 + c^2 + z^2 &= a^2 + \frac{c^2}{a^2}\rho^2 \\ \left( 1 - \frac{c^2}{a^2} \right)\rho^2 + z^2 &= a^2 - c^2 \\ \frac{\rho^2}{a^2} + \frac{z^2}{a^2-c^2} &= 1 \end{aligned}\]
Let \(b^2 = a^2 - c^2\) be the semi-minor axis and \(e^2 = \frac{c^2}{a^2} = 1 - \frac{b^2}{a^2}\) be the first eccentricity squared.
\[\begin{aligned} \frac{\rho^2}{a^2} + \frac{z^2}{b^2} &= 1 \\ \rho^2 + \frac{z^2}{b^2/a^2} &= a^2 \\ \rho^2 + \frac{z^2}{1-e^2} &= a^2 \end{aligned}\]
Let \(g(\rho,z) = 0\) constrain the curve.
\[\begin{aligned} g(\rho,z) &= \rho^2 + \frac{z^2}{1-e^2} - a^2 \end{aligned}\]
We want to find a set of orthogonal coordinates similar to polar coordinates on a circle but on an ellipse. This will be represented by the pair of 2D coordinates \((\phi, h)\) denoting (geodetic) latitude and altitude (geodetic height) respectively.
Note: \(h = 0\) on the ellipse, so it will not be present in the equations for the rest of this section!
We take the gradient of the curve to determine the orthogonal vector from the curve to the \(z\) axis. This is known as the prime vertical radius of curvature, \(N\), where \(\rho = N \cos\phi\).
\[\begin{aligned} \nabla g(\rho,z) &= \left( 2\rho, 2\frac{z}{1-e^2} \right) \\ \vec{n} &= \frac{1}{2} \nabla g(\rho,z) \\ &= \left( \rho, \frac{z}{1-e^2} \right) \\ \vec{n}' &= \frac{1-e^2}{2} \nabla g(\rho,z) \\ &= \left( \left( 1-e^2 \right)\rho, z \right) \\ (\rho,z) - \vec{n} &= \left( 0, \frac{-e^2}{1-e^2}z \right) \\ (\rho,z) - \vec{n}' &= \left( e^2 \rho, 0 \right) \\ N^2 &= \|\vec{n}\|^2 \\ &= \rho^2 + \frac{z^2}{(1-e^2)^2} \\ \vec{n} - \vec{n}' &= \left( e^2 \rho, \frac{e^2}{1-e^2}z \right) \\ &= e^2 \vec{n} \\ \Aboxed{\|\vec{n} - \vec{n}'\| &= e^2 N} \\ z^2 &= \left( 1-e^2 \right) \left( a^2 - \rho^2 \right) \\ N^2 &= \rho^2 + \frac{a^2 - \rho^2}{1-e^2} \\ &= \frac{1}{1-e^2}a^2 - \frac{e^2}{1-e^2}\rho^2 \\ \rho &= N \cos\phi \\ N^2 \left( \left( 1 - e^2 \right) + e^2 \cos^2\phi \right) &= a^2 \\ \cos^2\phi &= 1 - \sin^2\phi \\ N^2 \left( 1 - e^2 \sin^2\phi \right) &= a^2 \\ \Aboxed{N &= \frac{a}{\sqrt{1 - e^2 \sin^2\phi}}} \end{aligned}\]
Now we can caculate \((\rho, z)\) in terms of only \((\phi, h)\) and the constants \(a\) and \(e\).
\[\begin{aligned} \rho &= N \cos\phi \\ z &= \left( N - e^2 N \right) \sin\phi \\ &= \left( 1 - e^2 \right) N \sin\phi \end{aligned}\]
Earth's surface is approximated by an ellipsoid (not an ellipse), so we need to do a simple transformation, \(\rho^2 = x^2 + y^2\), to convert the ellipse into an ellipsoid. Another coordinate is added, \(\lambda\), called the geodetic longitude. This creates a sort of "spherical" coordinates, except on the reference ellipsoid.
\[x = \rho \cos\lambda\] \[y = \rho \sin\lambda\]
There are four defining parameters in the WGS 84 standardization document. We only need the first two for our transformations.
From these we can calculate other geometric constants.
We have shown how to formulate geodetic coordinates on the reference ellipsoid and formulate the transformations into their corresponding cartesian coordinates. On the reference ellipsoid, altitude is zero. Moving off of the reference ellipsoid is simply done by setting a non-zero altitude. These unrestricted coordinates are known as geocentric coordinates. Geocentric coodinates are also known as Earth-Centered Earth-Fixed (ECEF) coordinates. These are covered in the next section.
In the last section, we found the ECEF representation of the reference ellipsoid in terms of only latitude, longitude, and altitude. Now, we want to generalize this to any coordinate on or off of the ellipsoid. ECEF coordinates are called such because the Earth's center of mass is at the origin. They are also known as geocentric coordinates. To get coordinates off of the ellipsoid we only need a slight modification of the previous formulas.
\[\begin{aligned} \Aboxed{\rho &= (N + h) \cos\phi} \\ \Aboxed{x &= \rho \cos\lambda} \\ \Aboxed{y &= \rho \sin\lambda} \\ \Aboxed{z &= \left( \left( 1 - e^2 \right) N + h \right) \sin\phi} \end{aligned}\]
Implementations of the forward and reverse transformations can be seen here: https://hal.archives-ouvertes.fr/hal-01704943v2/document
We now want to find the plane tangent to the reference ellipsoid, shifted to a certain altitude. This is called the local tangent plane. The convention we will use for the tangent plane is North, East, and Down (NED) coordinates. Converting between ECEF coordinates and NED coordinates is a simple translation and rotation. Define \(\vec{N} = (n, e, d)^T\) and \(\vec{E} = (x, y, z)^T\).
Note: \(\vec{N}\) is not the same as \(N\) above.
\[\begin{aligned} \Aboxed{\vec{N} &= {}_E^N R \left( \vec{E} - \vec{E}_0 \right)} \\ {}_E^N R &= R_y(\phi + \pi / 2) R_z(-\lambda) \\ &= \begin{pmatrix} -\sin\phi & 0 & \cos\phi \\ 0 & 1 & 0 \\ -\cos\phi & 0 & -\sin\phi \end{pmatrix} \begin{pmatrix} \cos\lambda & \sin\lambda & 0 \\ -\sin\lambda & \cos\lambda & 0 \\ 0 & 0 & 1 \end{pmatrix} \\ &= \boxed{\begin{pmatrix} -\sin\phi\cos\lambda & -\sin\phi\sin\lambda & \cos\phi \\ -\sin\lambda & \cos\lambda & 0 \\ -\cos\phi\cos\lambda & -\cos\phi\sin\lambda & -\sin\phi \end{pmatrix}} \end{aligned}\]
Define yaw, pitch, and roll as \((\psi, \theta, \phi)\) respectively and \(\vec{B} = (b_x, b_y, b_z)^T\).
Note: \(\phi\) is not the same as latitude above.
\[\begin{aligned} \Aboxed{\vec{B} &= {}_N^B R \vec{N}} \\ {}_N^B R &= R_x(-\phi) R_y(-\theta) R_z(-\psi) \\ &= \begin{pmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & \sin\phi \\ 0 & -\sin\phi & \cos\phi \end{pmatrix} \begin{pmatrix} \cos\theta & 0 & -\sin\theta \\ 0 & 1 & 0 \\ \sin\theta & 0 & \cos\theta \end{pmatrix} \begin{pmatrix} \cos\psi & \sin\psi & 0 \\ -\sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{pmatrix} \\ &= \boxed{\begin{pmatrix} \cos\theta \cos\psi & \cos\theta \sin\psi & -\sin\theta \\ \sin\phi \sin\theta \cos\psi - \cos\phi \sin\psi & \sin\phi \sin\theta \sin\psi + \cos\phi \cos\psi & \sin\phi \cos\theta \\ \cos\phi \sin\theta \cos\psi + \sin\phi \sin\psi & \cos\phi \sin\theta \sin\psi - \sin\phi \cos\psi & \cos\phi \cos\theta \end{pmatrix}} \end{aligned}\]